#include "bsp_can.h"

/**
* @brief:      	BSP_CAN_Init
* @param:       void
* @retval:     	void
**/
void BSP_CAN_Init(void)
{ 
	can_filter_init(); 
}

/**
* @brief:      	CAN_Filter_Init
* @param:       void
* @retval:     	void
* @details:    	CAN滤波器初始化
**/
uint8_t can_filter_init_error = 0;
void CAN_Filter_Init(void)
{
	CAN_FilterTypeDef can_filter_st;
	can_filter_st.FilterActivation = ENABLE;
	can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
	can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
	can_filter_st.FilterIdHigh = 0x0000;
	can_filter_st.FilterIdLow = 0x0000;
	can_filter_st.FilterMaskIdHigh = 0x0000;
	can_filter_st.FilterMaskIdLow = 0x0000;
	can_filter_st.FilterBank = 0;
	can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
	if(HAL_CAN_ConfigFilter(&hcan1, &can_filter_st) != HAL_OK) {
		can_filter_init_error = 1;
  }
	// 确保CAN处于正常模式
	if(HAL_CAN_Start(&hcan1) != HAL_OK) {
		can_filter_init_error = 2;
	}
	// 激活接收中断
	if(HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
		can_filter_init_error = 3;
	}
}

/**
* @brief:      	BSP_CAN_SendData
* @param:       hcan:								CAN句柄
* @param:       id: 								CAN设备ID
* @param:       data: 							发送的数据
* @param:       len:  							发送的数据长度
* @retval:     	void
* @details:    	发送数据
**/
uint8_t BSP_CAN_SendData(CAN_HandleTypeDef *hcan, uint16_t id, uint8_t *data, uint32_t len)
{	
	CAN_TxHeaderTypeDef	tx_header;
	
    uint16_t t = 0; //用于计时，放置发送超时
 
    // 安全断言
    if(len > 8 || data == NULL) return 1;
    
    // 分别处理标准帧和扩展帧
    if(id <= 0x7FF){
      tx_header.StdId = id & 0x7FF;         /* 标准标识符,确保11位ID */
      tx_header.ExtId = 0; 
      tx_header.IDE = CAN_ID_STD;
    }
    else{
      tx_header.ExtId = id & 0x1FFFFFFF;    /* 扩展标识符(29位) */
      tx_header.StdId = 0; 
      tx_header.IDE = CAN_ID_EXT;
    }
    tx_header.RTR = CAN_RTR_DATA; /* 数据帧 */
    tx_header.DLC = len;
	
	// 找到空的发送邮箱，把数据发送出去
	if(HAL_CAN_AddTxMessage(hcan, &tx_header, data, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) {
		if(HAL_CAN_AddTxMessage(hcan, &tx_header, data, (uint32_t*)CAN_TX_MAILBOX1) != HAL_OK) {
			if(HAL_CAN_AddTxMessage(hcan, &tx_header, data, (uint32_t*)CAN_TX_MAILBOX2) != HAL_OK){
				return 1;
			}
		}
	}

	//等待发送完成
    while (HAL_CAN_GetTxMailboxesFreeLevel(hcan) != 3)   /* 等待发送完成,所有邮箱为空 */
    {
      t++;
      if (t > 0x100)
      {
        return 1; //发送超时
      }
    }
    return 0;  //发送成功且未超时
}

/**
* @brief:      	HAL_CAN_RxFifo0MsgPendingCallback
* @param:       hfdcan；FDCAN句柄
* @param:       RxFifo0ITs：中断标志位
* @retval:     	void
* @details:    	HAL库的CAN中断回调函数
**/
volatile uint8_t can_rx_data[6][8] = {{0}};
uint32_t can_callback_cnt = 0;
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef rx_header;
  uint8_t rx_data[8] = {0};
	if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) {
		if (rx_header.RTR == CAN_RTR_DATA && rx_header.DLC > 0) {
			// debug
			HAL_GPIO_TogglePin(GPIOH, GPIO_PIN_11);
			can_callback_cnt++;
			
			uint32_t raw_id = 0;
			// 获取ID（标准和扩展帧）
			if (rx_header.IDE == CAN_ID_STD) {
				raw_id = rx_header.StdId;    // 标准帧ID
			} else {
				raw_id = rx_header.ExtId;    // 扩展帧ID
			}
			// 针对不同电机或编码器做处理
			uint8_t index = 0;
			switch(raw_id) {
				case MOTOR_RX_ID_1: index = 0; break;
				case MOTOR_RX_ID_2: index = 1; break;
				case MOTOR_RX_ID_3: index = 2; break;
				case MOTOR_RX_ID_4: index = 3; break;
				case MOTOR_RX_ID_5: index = 4; break;
				case MOTOR_RX_ID_6: index = 5; break;
				default: return;
			}
			for(uint8_t i = 0; i < 8; i++) {
				can_rx_data[index][i] = rx_data[i];
			}
		}
	}
}

uint32_t can_errorcode = 0;
HAL_CAN_StateTypeDef can_state;
uint8_t can_mode = 0;
uint32_t can_instace_ier;
uint32_t can_instance_esr;
void BSP_CAN_CheckStatus(void)
{
	can_state = HAL_CAN_GetState(&hcan1);
	can_errorcode = HAL_CAN_GetError(&hcan1);
	(hcan1.Instance->MCR & CAN_MCR_SLEEP) ? (can_mode = 1) : (can_mode = 2);
	can_instace_ier = hcan1.Instance->IER;
	can_instance_esr = hcan1.Instance->ESR;
}

/**
* @brief:      	BSP_CAN_Reset
* @param:       void
* @retval:     	void
* @details:    	重置CAN模块
**/
void BSP_CAN_Reset(void)
{
	// 停止CAN
	HAL_CAN_Stop(&hcan1);
	
	// 延时确保完全停止
	HAL_Delay(10);
	
	// 重新初始化
	MX_CAN1_Init();
	BSP_CAN_Init();
}

/**
* @brief:      	BSP_CAN_GetDetailedStatus
* @param:       void
* @retval:     	void
* @details:    	获取详细CAN状态
**/
volatile uint32_t debug_msr ;
volatile uint32_t debug_tsr ;
volatile uint32_t debug_rf0r;
volatile uint32_t debug_rf1r;
void BSP_CAN_GetDetailedStatus(void)
{
    BSP_CAN_CheckStatus();
    
    // 检查更多寄存器状态
    uint32_t msr = hcan1.Instance->MSR;
    uint32_t tsr = hcan1.Instance->TSR;
    uint32_t rf0r = hcan1.Instance->RF0R;
    uint32_t rf1r = hcan1.Instance->RF1R;
    
    // 这些值可以在调试时观察
    debug_msr = msr;
    debug_tsr = tsr;
    debug_rf0r = rf0r;
    debug_rf1r = rf1r;
}

void can_filter_init(void)
{
	CAN_FilterTypeDef can_filter_st;
	can_filter_st.FilterActivation = ENABLE;
	can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
	can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
	can_filter_st.FilterIdHigh = 0x0000;
	can_filter_st.FilterIdLow = 0x0000;
	can_filter_st.FilterMaskIdHigh = 0x0000;
	can_filter_st.FilterMaskIdLow = 0x0000;
	can_filter_st.FilterBank = 0;
	can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
	HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
	HAL_CAN_Start(&hcan1);
	HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
}

uint8_t canx_send_data(CAN_HandleTypeDef *hcan, uint16_t id, uint8_t *data, uint32_t len)
{	
	CAN_TxHeaderTypeDef	tx_header;
	
	tx_header.StdId = id;
	tx_header.ExtId = 0;
	tx_header.IDE   = 0;
	tx_header.RTR   = 0;
	tx_header.DLC   = len;
  /*找到空的发送邮箱，把数据发送出去*/
	if(HAL_CAN_AddTxMessage(hcan, &tx_header, data, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) {
		if(HAL_CAN_AddTxMessage(hcan, &tx_header, data, (uint32_t*)CAN_TX_MAILBOX1) != HAL_OK) {
			HAL_CAN_AddTxMessage(hcan, &tx_header, data, (uint32_t*)CAN_TX_MAILBOX2);
    }
  }
  return 0;
}


void set_motor_voltage(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
{
  CAN_TxHeaderTypeDef tx_header;
  uint8_t             tx_data[8];
    
  tx_header.StdId = (id_range == 0)?(0x1ff):(0x2ff);
  tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
  tx_data[2] = (v2>>8)&0xff;
  tx_data[3] =    (v2)&0xff;
  tx_data[4] = (v3>>8)&0xff;
  tx_data[5] =    (v3)&0xff;
  tx_data[6] = (v4>>8)&0xff;
  tx_data[7] =    (v4)&0xff;
  HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}
